/*#pragma DATA_SECTION(imm, "SRAMB");
unsigned short imm[6497]; 
unsigned short *im=&imm[0]; */
#include <stdio.h>

int counter=0;
int turning_left=0;
int turning_right=0;
int approaching_left=0;
int approaching_right=0;
int yprev;
int extradist=100;
int change;
int prevseg[4]={178,0,178,150};
int *prev=&prevseg[0];


void detection(unsigned short *image, unsigned short * im)
{
//printf("detection\n");
  double data[20];
  double* datapointer;
  //int* imagepointer;
  int ytarget=200;
  int prevy=180;
  //unsigned short imm[6497];
  
  threshfuncv2(image,im);
  datapointer=&data[0];
  //printf("we get here\n");
  getxtarget(datapointer,ytarget,prev,im);

  if((turning_left==1)||(turning_right==1)){
    if((data[2]==-1)||(abs(90-data[3])>10)){//no vertical line or not yet aligned
      	if(turning_left){
	 		control(0,0,1,1700);
		}
		if(turning_right){
			 control(0,0,2,1700);
		}
    }
    else{//return to tracking mode
      turning_right=0;
      turning_left=0;
      control(data[1],data[2],0,1700);
    }
  }
  else
  { if(data[2]==0) //no turns or intersections
  	{
    	control(data[0],data[1],0,1700);
	}
    if(data[2]==1)    //sharp turn detected
	{
      change=data[4]-yprev;
      approaching_left=1;
      control(data[4],data[5],0,1700);//steer towards intersection
      prevy=data[4];
	}
    if(data[2]==2)//sharp turn detected
	{
      change=data[4]-yprev;
      approaching_right=1;
      control(data[4],data[5],0,1700);//steer towards intersection
      prevy=data[4];
	  
    }
    if(data[2]==-1){//no vertical line
      if(approaching_left||approaching_right){
        if(extradist>0){
		  control(178,200,0,1700);
		  extradist=extradist-change;
        }
        else{
	  		extradist=100;//reset
	  		if(approaching_right==1){
	    		turning_right=1;
	    		approaching_right=0;
	  		} 
	  		if(approaching_left==1){
	    			turning_left=1;
	    			approaching_left=0;
	  		}
        }
      }
	  else{//do nothing
	//    control(178,200,0,1700);
	      control(0,0,3,0);
	  }
    }
}
//control(178,200,0,700);

}

    